#include "algo_math.h"
#include "algo_buffer.h"


// MACRO
#define INDEX(buf, i)       ( ( (buf)->u32_pointer + (buf)->u32_length - (i) ) % (buf)->u32_length )
#define DATA(buf, i)        ( *( (buf)->i32p_data + INDEX(buf, i) ) )


// Function definition

void Buffer_Init(cycle_buffer *tp_buffer, int32 *i32p_data, uint32 u32_length)
{
    tp_buffer->i32p_data = i32p_data;
    tp_buffer->u32_length = u32_length;
    Buffer_Reset(tp_buffer);
}


void Buffer_Reset( cycle_buffer *tp_buffer )
{
    tp_buffer->u32_pointer = tp_buffer->u32_length-1;
    tp_buffer->u32_count = 0;
}


void Buffer_Push( cycle_buffer *tp_buffer, int32 i32_value )
{
    tp_buffer->u32_pointer++;
    tp_buffer->u32_pointer %= tp_buffer->u32_length;
    *( tp_buffer->i32p_data + tp_buffer->u32_pointer ) = i32_value;
    if (tp_buffer->u32_count < tp_buffer->u32_length)
        tp_buffer->u32_count++;
}


void Buffer_Pop( cycle_buffer *tp_buffer )
{
    if (tp_buffer->u32_count > 0)
        tp_buffer->u32_count--;
}


int32 Buffer_Get( const cycle_buffer *tp_buffer, uint32 u32_index )
{
    if ( u32_index >= tp_buffer->u32_count )
        return 0;
    return DATA(tp_buffer, u32_index);
}

void Buffer_Replace( const cycle_buffer *tp_buffer, uint32 u32_index, int32 i32_value )
{
    if ( u32_index < tp_buffer->u32_count )
        DATA(tp_buffer, u32_index) = i32_value;
}

uint32 Buffer_Count( const cycle_buffer *tp_buffer, uint32 u32_num  )
{
    uint32 i;
    uint32 u32_ret = 0;

    u32_num = F_MIN(u32_num, tp_buffer->u32_count);
    for( i=0; i<u32_num; i++ )
    {
        u32_ret += DATA(tp_buffer, i) ? 1 : 0;
    }
    return u32_ret;
}

int32 Buffer_Diff( const cycle_buffer *tp_buffer, uint32 u32_index )
{
    if ( u32_index+1 >= tp_buffer->u32_count )
        return 0;
    return DATA(tp_buffer, u32_index) - DATA(tp_buffer, u32_index+1);
}


int32 Buffer_Mean( const cycle_buffer *tp_buffer, uint32 u32_num )
{
    return Mean(tp_buffer, 0, u32_num, L_FALSE);
}

int32 Buffer_AbsMean( const cycle_buffer *tp_buffer, uint32 u32_num )
{
    return Mean(tp_buffer, 0, u32_num, L_TRUE);
}

int32 Mean( const cycle_buffer *tp_buffer,
                   uint32 u32_first,
                   uint32 u32_last,
                   logical t_abs )
{
    uint32 i;
    int32 i32_data, i32_ret = 0;

    u32_last = F_MIN(u32_last, tp_buffer->u32_count);
    if (u32_last <= u32_first)
        return 0;

    for( i=u32_first; i<u32_last; i++ )
    {
        i32_data = DATA(tp_buffer, i);
        i32_ret += t_abs ? ABS(i32_data) : i32_data;
    }
    i32_ret = S_DIV(i32_ret, (int32)(u32_last-u32_first));

    return i32_ret;
}

int32 Buffer_MeanOfMinN(const cycle_buffer *tp_buffer, int32 *i32p_temp, uint32 u32_first , uint32 u32_last)
{
    int32 i32_ret = 0;
    int32 i = 0;

    u32_last = F_MIN(u32_last, tp_buffer->u32_count);
    if (u32_first >= u32_last)
        u32_first = 0;
    for(i=0; i<tp_buffer->u32_count; i++)
        i32p_temp[i] = DATA(tp_buffer, i);
    QuickSort(i32p_temp, 0, tp_buffer->u32_count-1);

    for( i=u32_first; i<u32_last; i++ )
        i32_ret += i32p_temp[i];
    i32_ret = U_DIV(i32_ret, u32_last - u32_first);

    return i32_ret;
}

int32 Buffer_MeanOfMaxN(const cycle_buffer *tp_buffer, int32 *i32p_temp, uint32 u32_first , uint32 u32_last)
{
    int32 i32_ret = 0;
    int32 i = 0;

    u32_last = F_MIN(u32_last, tp_buffer->u32_count);
    if (u32_first >= u32_last)
        u32_first = 0;
    for(i=0; i<tp_buffer->u32_count; i++)
        i32p_temp[i] = DATA(tp_buffer, i);
    QuickSort(i32p_temp, 0, tp_buffer->u32_count-1);

    for( i=tp_buffer->u32_count-u32_first-1; i>=tp_buffer->u32_count-u32_last; i-- )
        i32_ret += i32p_temp[i];
    i32_ret = U_DIV(i32_ret, u32_last - u32_first);

    return i32_ret;
}

int32 Var( const cycle_buffer *tp_buffer,
                   uint32 u32_first,
                   uint32 u32_last)
{
    uint32 i;
    int32 i32_data, i32_mean = 0, i32_ret = 0;

    u32_last = F_MIN(u32_last, tp_buffer->u32_count);
    if (u32_last <= u32_first)
        return 0;

    for( i=u32_first; i<u32_last; i++ )
    {
        i32_data = DATA(tp_buffer, i);
        i32_mean += i32_data;
    }
    i32_mean = S_DIV(i32_mean, (int32)(u32_last-u32_first));

    for( i=u32_first; i<u32_last; i++ )
    {
        i32_data = DATA(tp_buffer, i);
        i32_data = i32_data - i32_mean;
        i32_data = F_SQR(i32_data);
        i32_ret += i32_data;
    }
    i32_ret = S_DIV(i32_ret, (int32)(u32_last-u32_first));

    return i32_ret;
}


/*
// Buffer_TrimedMean will change the data of buffer
int32 Buffer_TrimedMean( cycle_buffer *tp_buffer, uint32 u32_trimPercent )
{
    uint32 u32_trimLength;
    u32_trimLength = tp_buffer->u32_length * u32_trimPercent / HUNDRED_PERCENT;

    Buffer_Sort(tp_buffer);
    return Mean(tp_buffer, u32_trimLength, tp_buffer->u32_count-u32_trimLength, L_FALSE);
}

// Buffer_Sort will change the data of buffer
void Buffer_Sort(cycle_buffer *tp_buffer)
{
    QuickSort(tp_buffer->i32p_data, 0, tp_buffer->u32_count-1);
}
*/


int32 Buffer_TrimedMean( const cycle_buffer *tp_buffer, uint32 u32_num )
{
    uint32 i;
    int32 i32_data,
          i32_ret = 0,
          i32_max = 0x80000000,
          i32_min = 0x7FFFFFFF;

    u32_num = F_MIN( u32_num, tp_buffer->u32_count );

    for( i=0; i<u32_num; i++ )
    {
        i32_data = DATA( tp_buffer, i );
        if ( i32_data > i32_max )
            i32_max = i32_data;
        if ( i32_data < i32_min )
            i32_min = i32_data;
        i32_ret += i32_data;
    }
    if (u32_num>6)
    {
        i32_ret -= i32_max+i32_min;
        u32_num -= 2;
    }
    i32_ret = S_DIV( i32_ret, (int32)u32_num );
    return i32_ret;
}

int32 Buffer_TrimedVar( const cycle_buffer *tp_buffer, uint32 u32_num )
{
    uint32 i;
    int32 i32_data,
          i32_mean = 0,
          i32_ret = 0,
          i32_maxi = -1,
          i32_mini = -1,
          i32_max = 0x80000000,
          i32_min = 0x7FFFFFFF;

    u32_num = F_MIN( u32_num, tp_buffer->u32_count );

    for( i=0; i<u32_num; i++ )
    {
        i32_data = DATA( tp_buffer, i );
        if ( i32_data > i32_max )
        {
            i32_max = i32_data;
            i32_maxi = i;
        }
        if ( i32_data < i32_min )
        {
            i32_min = i32_data;
            i32_mini = i;
        }
        i32_mean += i32_data;
    }
    if (u32_num>6)
    {
        i32_mean -= i32_max+i32_min;
        i32_mean = S_DIV( i32_mean, (int32)u32_num-2 );
    }
    else
    {
        i32_mean = S_DIV( i32_mean, (int32)u32_num );
    }

    for( i=0; i<u32_num; i++ )
    {
        if (u32_num>6 && (i==i32_maxi || i==i32_mini))
            continue;
        i32_data = DATA( tp_buffer, i );
        i32_data = i32_data - i32_mean;
        i32_data = F_SQR(i32_data);
        i32_ret += i32_data;
    }
    if (u32_num>6)
    {
        i32_ret = S_DIV( i32_ret, (int32)u32_num-2 );
    }
    else
    {
        i32_ret = S_DIV( i32_ret, (int32)u32_num );
    }

    return i32_ret;
}

int32 Buffer_MeanAcc( const cycle_buffer *tp_buffer, uint32 u32_num )
{
    uint32 i;
    int32 i32_data,
          i32_ret = 0;

    for( i=0; i<u32_num-2; i++ )
    {
        i32_data = DATA( tp_buffer, i+2 ) + DATA( tp_buffer, i ) - DATA( tp_buffer, i+1) * 2;
        i32_data = ABS(i32_data);
        i32_data = F_MIN(i32_data, 5*PERCISION);
        i32_ret += i32_data;
    }

    i32_ret = S_DIV( i32_ret, (int32)u32_num-2 );

    return i32_ret;
}


int32 Buffer_Filter( const cycle_buffer *tp_buffer,
                     const filter_window *tp_window )
{
    uint32 i,j;
    int32 i32_ret = 0, i32_coefSum = 0;

    j = F_MIN( tp_buffer->u32_count, tp_window->u32_length );
    for ( i=0 ; i<j; i++ )
    {
        i32_ret += DATA(tp_buffer, i) * tp_window->i32p_coefficient[i];
        i32_coefSum += tp_window->i32p_coefficient[i];
    }
    if ( tp_window->i32_magnification != 0 )
        i32_coefSum = tp_window->i32_magnification;
    else if ( i32_coefSum == 0 )
        return 0;
    i32_ret = S_DIV( i32_ret, i32_coefSum );
    return i32_ret;
}

int32 Buffer_TrimedFilter( const cycle_buffer *tp_buffer,
                           const filter_window *tp_window,
                           int32 i32_upper,
                           int32 i32_lower )
{
    uint32 u32_count;
    uint8 u8_validCount = 0,
          u8_imax = 0,
          u8_imin = 0;
    int32 i32_data = 0,
          i32_max = 0x80000000,
          i32_min = 0x7FFFFFFF,
          i32_diff = 0,
          i32_ret = 0,
          i32_coefSum = 0;
    logical t_valid = L_TRUE,
	        t_validLast = L_TRUE;

    u32_count = F_MIN( tp_buffer->u32_count, tp_window->u32_length );
    if (u32_count==0)
        return 0;

    int i;
    for ( i=u32_count-1; i>=0; i-- )
    {
        i32_data = DATA(tp_buffer, i);
		t_valid = i32_data < i32_upper && i32_data > i32_lower;
        if (t_valid && t_validLast)
        {
            u8_validCount++;
            if ( i32_data > i32_max )
            {
                i32_max = i32_data;
                u8_imax = i;
            }
            if ( i32_data < i32_min )
            {
                i32_min = i32_data;
                u8_imin = i;
            }
            if (i>0)
            {
                i32_diff += ABS(i32_data - DATA(tp_buffer, i-1));
            }
            i32_ret += i32_data * tp_window->i32p_coefficient[i];
            i32_coefSum += tp_window->i32p_coefficient[i];
        }
        t_validLast = t_valid;
    }
    if ( u8_validCount >= 6 &&
         u8_validCount >= u32_count*2/3 &&
         i32_max-i32_min > i32_diff*3/((int32)u8_validCount-1) )
    {
        i32_ret -= DATA(tp_buffer, u8_imax) * tp_window->i32p_coefficient[u8_imax]
                 + DATA(tp_buffer, u8_imin) * tp_window->i32p_coefficient[u8_imin];
        i32_coefSum -= tp_window->i32p_coefficient[u8_imax]
                     + tp_window->i32p_coefficient[u8_imin];
    }
    if ( tp_window->i32_magnification != 0 )
        i32_coefSum = tp_window->i32_magnification;
    else if ( i32_coefSum == 0 )
        return 0;
    i32_ret = S_DIV( i32_ret, i32_coefSum );
    return i32_ret;
}

void Boxcar_Init( boxcar *tp_boxcar, uint32 u32_size )
{
    tp_boxcar->i32_min = 0x7FFFFFFF;
    tp_boxcar->i32_max = 0x80000000;
    tp_boxcar->u32_count = 0;
    tp_boxcar->u32_size = u32_size;
    tp_boxcar->i32_sum = 0;
}

void Boxcar_Add( boxcar *tp_boxcar, int32 i32_value )
{
    if (tp_boxcar->u32_count > tp_boxcar->u32_size * 2)
        Boxcar_Init(tp_boxcar, tp_boxcar->u32_size);

    if ( i32_value > tp_boxcar->i32_max )
        tp_boxcar->i32_max = i32_value;
    if ( i32_value < tp_boxcar->i32_min )
        tp_boxcar->i32_min = i32_value;

    tp_boxcar->i32_sum += i32_value;
    tp_boxcar->u32_count++;
}

logical Boxcar_IsFull(boxcar *tp_boxcar)
{
    return tp_boxcar->u32_count >= tp_boxcar->u32_size;
}

int32 Boxcar_Output(boxcar *tp_boxcar)
{
    int32 i32_ret = 0;
    if (tp_boxcar->u32_count >= 10)
    {
        tp_boxcar->i32_sum -= tp_boxcar->i32_max + tp_boxcar->i32_min;
        tp_boxcar->u32_count -= 2;
    }
    i32_ret = S_DIV( tp_boxcar->i32_sum, (int32)tp_boxcar->u32_count );
    Boxcar_Init(tp_boxcar, tp_boxcar->u32_size);
    return i32_ret;
}
